package roborl.snakeBot;

import java.util.HashMap;
import java.util.Vector;

import net.phys2d.math.Vector2f;
import net.phys2d.raw.Body;
import net.phys2d.raw.FixedMotorJoint;
import net.phys2d.raw.shapes.Box;

import roborl.Action;
import roborl.Decision;
import roborl.DiscreteValueApr;
import roborl.PhysRobot;
import roborl.PhysicsWorld;
import roborl.RobotState;
import roborl.StaticState;
import roborl.triaLegRobo.MonsterReward;
import roborl.triaLegRobo.ThreeLegMonster;

public class SnakeRobot extends PhysRobot {
	DiscreteValueApr apr;
	Body b1, b2, b3, b4;
	FixedMotorJoint m1, m2, m3;
	Vector<Action> allActions;
	{
		allActions = new Vector<Action>();
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				for (int k = 0; k < 3; k++) {
					allActions.add(new MultiAction(i, j, k));
				}
			}
		}
	}

	static class MultiAction extends Action {
		public int direction[];

		public MultiAction(int... i) {
			super("-" + i[0] + "-" + i[1] + "-" + i[2]);
			direction = i;
		}
	}

	public SnakeRobot() {
		super.DECISION_ITERATION = 40;
		super.SILENT_ITERATION = 0;
		apr = new DiscreteValueApr(allActions);
		Decision dec = new Decision(apr, new SnakeReward());
		setDecision(dec);

		float x = 250;
		float y = 200;

		int length, width;
		length = 50;
		width = 10;
		b1 = new Body(new Box(length, width), 1);
		b1.setPosition(x - length, y - width / 2);
		b2 = new Body(new Box(length, width), 1);
		b2.setPosition(x, y - width / 2);
		b3 = new Body(new Box(length, width), 1);
		b3.setPosition(x + length, y - width / 2);
		b4 = new Body(new Box(length, width), 1);
		b4.setPosition(x + length * 2, y - width / 2);
		b1.setFriction(1.5f);
		b2.setFriction(1.5f);
		b3.setFriction(1.5f);
		b4.setFriction(1.5f);
		m1 = new FixedMotorJoint(b1, b2, new Vector2f((float) length / 2f, 0),
				new Vector2f(-(float) length / 2f, 0));

		m2 = new FixedMotorJoint(b2, b3, new Vector2f((float) length / 2f, 0),
				new Vector2f(-(float) length / 2f, 0));
		m3 = new FixedMotorJoint(b3, b4, new Vector2f((float) length / 2f, 0),
				new Vector2f(-(float) length / 2f, 0));

		Vector<Float> set = new Vector<Float>();
		set.add(-1f);
		set.add(0f);
		set.add(1f);

		m1.setAngleSet(set);
		m2.setAngleSet(set);
		m3.setAngleSet(set);

		m1.setAngularSpeed(0.4f);
		m2.setAngularSpeed(0.4f);
		m3.setAngularSpeed(0.4f);

		Vector<Object> parts = getParts();
		parts.add(b1);
		parts.add(b2);
		parts.add(b3);
		parts.add(b4);

		parts.add(m1);
		parts.add(m2);
		parts.add(m3);

	}

	private double getCenter() {
		double c = 0;
		c += b1.getPosition().getX();
		c += b2.getPosition().getX();
		c += b3.getPosition().getX();
		c += b4.getPosition().getX();
		c /= 4;
		return c;
	}

	@Override
	public Vector<Action> getAvailableActions() {
		// TODO Auto-generated method stub
		Vector<Action> allActions = new Vector<Action>();
		for (int i = 0; i < 3; i++) {
			if (m1.getCurrentAngleSet() == 0 && i == 0)
				continue;
			if (m1.getCurrentAngleSet() == 2 && i == 1)
				continue;
			for (int j = 0; j < 3; j++) {
				if (m2.getCurrentAngleSet() == 0 && j == 0)
					continue;
				if (m2.getCurrentAngleSet() == 2 && j == 1)
					continue;
				for (int k = 0; k < 3; k++) {
					if (m3.getCurrentAngleSet() == 0 && k == 0)
						continue;
					if (m3.getCurrentAngleSet() == 2 && k == 1)
						continue;
					MultiAction action = new MultiAction(i, j, k);
					allActions.add(action);
				}
			}
		}
		// allActions.add(none);
		return allActions;
	}

	@Override
	public RobotState getState() {
		// TODO Auto-generated method stub
		// int subDivide = 4;
		HashMap<String, Double> s = new HashMap<String, Double>();
		// s.put("m1", (double) (int) (mod(b1.getRotation(), 2 * Math.PI) / 2
		// / Math.PI * subDivide));
		// s.put("m1", (double) (int) (mod(b2.getRotation(), 2 * Math.PI) / 2
		// / Math.PI * subDivide));
		// s.put("m1", (double) (int) (mod(b3.getRotation(), 2 * Math.PI) / 2
		// / Math.PI * subDivide));
		s.put("m1", (double) m1.getCurrentAngleSet());
		s.put("m2", (double) m2.getCurrentAngleSet());
		s.put("m3", (double) m3.getCurrentAngleSet());

		Vector<Double> hs = new Vector<Double>();
		hs.add(getCenter());
		return new StaticState(s, hs);
	}

	@Override
	protected void simpleUpdate() {
		// TODO Auto-generated method stub
		double center = getCenter();
		if (center > 700) {
			lastAction = null;
			for (Object o : parts) {
				if (o instanceof Body) {
					((Body) o).adjustPosition(new Vector2f(-300, 00));
				}

			}
			achieve++;

		} else if (center < 100) {
			lastAction = null;
			for (Object o : parts) {
				if (o instanceof Body) {
					((Body) o).adjustPosition(new Vector2f(300, 00));
				}
			}

		}
	}

	@Override
	public void takeAction(Action action) {
		// TODO Auto-generated method stub
		if (action == none)
			return;
		MultiAction action2 = (MultiAction) action;
		if (action2.direction[0] == 0) {
			int ca = m1.getCurrentAngleSet();
			ca--;
			if (ca < 0)
				ca = 0;
			m1.setCurrentAngleSet(ca);
		} else if (action2.direction[0] == 1) {
			int ca = m1.getCurrentAngleSet();
			ca++;
			if (ca > 2)
				ca = 2;
			m1.setCurrentAngleSet(ca);
		} else {

		}

		if (action2.direction[1] == 0) {
			int ca = m2.getCurrentAngleSet();
			ca--;
			if (ca < 0)
				ca = 0;
			m2.setCurrentAngleSet(ca);
		} else if (action2.direction[1] == 1) {
			int ca = m2.getCurrentAngleSet();
			ca++;
			if (ca > 2)
				ca = 2;
			m2.setCurrentAngleSet(ca);
		} else {

		}

		if (action2.direction[2] == 0) {
			int ca = m3.getCurrentAngleSet();
			ca--;
			if (ca < 0)
				ca = 0;
			m3.setCurrentAngleSet(ca);
		} else if (action2.direction[2] == 1) {
			int ca = m3.getCurrentAngleSet();
			ca++;
			if (ca > 2)
				ca = 2;
			m3.setCurrentAngleSet(ca);
		} else {

		}
	}

	/**
	 * @param args
	 * @throws InterruptedException
	 */
	public static void main(String[] args) throws InterruptedException {
		// TODO Auto-generated method stub
		final PhysicsWorld physicsWorld = new PhysicsWorld();
		PhysRobot physRobot = new SnakeRobot();
		// physRobot.readOnly=true;
		physRobot.autoSave = true;
		physRobot.readOnly = false;
		new Thread() {
			public void run() {
				physicsWorld.start();
			}
		}.start();
		physicsWorld.target = 1;
		physRobot.read();
		Thread.sleep(1000);
		physicsWorld.addRobot(physRobot);
	}

}
